//
// Created by Aron on 2024/10/31.
//

#include "Pitch.h"
#include "Speed_control.h"
#include "CAN_receive.h"
#include "remote_control.h"

pid_type_def pid_speed_Pitch;
pid_type_def pid_angle_Pitch;
static float n_P=4.3945f;
static int16_t max_speed_change_P=2000;
int PitchFlag=1;
static float hlimit_P;
static float llimit_P;
static float set_angle;
static float ref_angle;
void Pitch_control(int16_t angle) {
    if (rc_ctrl.rc.s[0]!=1) {
        if (PitchFlag) {
            set_angle = motor_measure[2].total_ecd * n_P;
            PitchFlag = 0;
        } else set_angle += angle;
        ref_angle = motor_measure[2].total_ecd * n_P;
//    if (set_angle>hlimit_P) {
//        set_angle=hlimit_P;
//    }else if(set_angle<llimit_P) {
//        set_angle=llimit_P;
//    }
        PID_calc(&pid_angle_Pitch, ref_angle, set_angle);
        Speed_out(pid_angle_Pitch.out, motor_measure[2].speed_rpm, max_speed_change_P,
                  &pid_speed_Pitch);
    }else {
        ref_angle = motor_measure[2].total_ecd * n_P;
        PID_calc(&pid_angle_Pitch, ref_angle, set_angle);
        Speed_out(pid_angle_Pitch.out, motor_measure[2].speed_rpm, max_speed_change_P,
                  &pid_speed_Pitch);
    }
}